﻿using System;
using System.Drawing;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;

using webcam = Microsoft.Robotics.Services.WebCam.Proxy;
using lrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
//using simjaguar = SimJaguar.Jaguar.Proxy;

namespace SimJaguarsControl
{
    /// <summary>
    /// SimJaguarsControl state
    /// </summary>
    [DataContract]
    public class SimJaguarsControlState
    {
        private SimRobot[] _SimJaguar = new SimRobot[4];

        public void Initialize()
        {
            for (int i = 1; i < _SimJaguar.Length; i++)
                _SimJaguar[i] = new SimRobot();
        }

        [DataMember]
        public SimRobot[] SimJaguar
        {
            get { return _SimJaguar; }
            set { _SimJaguar = value; }
        }


    }

    [DataContract]
    public class SimRobot
    {

        private SimSensors _Sensors = new SimSensors();

        [DataMember]
        public SimSensors Sensors
        {
            get { return _Sensors; }
            set { _Sensors = value; }
        }

        private Physics _Physics = new Physics();

        [DataMember]
        public Physics Pose
        {
            get { return _Physics; }
            set { _Physics = value; }
        }

        #region SLAM and VFH
        private Map map = new Map();

        [DataMember]
        [Description("The objects related to simultaneous localization and mapping")]
        public Map MAP
        {
            get { return map; }
            set { map = value; }
        }

        private VFH vfh = new VFH();

        [DataMember]
        [Description("The objects related to VFH")]
        public VFH VFH
        {
            get { return vfh; }
            set { vfh = value; }
        }

        #endregion

        private Waypoints _waypoints = new Waypoints();

        [DataMember]
        [Description("For determining the traversed path and having a count of the times passed over it")]
        public Waypoints Waypoints
        {
            get { return _waypoints; }
            set { _waypoints = value; }
        }
    }

    [DataContract]
    public class SimSensors
    {
        [DataMember]
        public webcam.QueryFrameResponse WebcamData;

        [DataMember]
        public Image WebcamBitmap;

        [DataMember]
        public lrf.State laserData;

    }

    [DataContract]
    public class Physics
    {
        [DataMember]
        public double Speed;

        [DataMember]
        public double Roll;

        [DataMember]
        public double Pitch;

        [DataMember]
        public double Yaw;

        [DataMember]
        public double X_pose;

        [DataMember]
        public double Y_pose;

        [DataMember]
        public double Z_pose;

        [DataMember]
        public double LeftFrontSpeed;

        [DataMember]
        public double LeftRearSpeed;

        [DataMember]
        public double RightFrontSpeed;

        [DataMember]
        public double RightRearSpeed;

    }


    #region SLAM and VFH Classes
    [DataContract]
    public enum MapMode
    {
        Overwrite,      // Just write new data (free or occupied) into the map
        Counting       // Count the number of times a cell is seen free (or occ)
        //BayesRule       // Use Bayes Rule to update cell values
    }

    [DataContract]
    public class Map
    {
        [DataMember]
        public double MapWidth = 36;
        [DataMember]
        public double MapHeight = 28;
        [DataMember]
        public double MapResolution = 0.15;
        [DataMember]
        public double MapMaxRange = 2.99;
        [DataMember]
        public bool DrawMap = true;
        [DataMember]
        public MapMode DrawingMode = MapMode.Counting;
    }

    [DataContract]
    public class VFH
    {
        [DataMember]
        public bool DrawVFH = true;
        [DataMember]
        public int Thresh = 1000; //mm Same as AwareofObstacle distance.
        [DataMember]
        public int Min4Candid = 100; //Consecutive measurements to form a candidate.
    }
    #endregion

    [DataContract]
    public class Waypoints
    {
        [DataMember]
        public DateTime LastTime = new DateTime();

        [DataMember]
        public List<Points> Points = new List<Points>();

        [DataMember]
        public Tuple<bool, int> LastPoint = new Tuple<bool, int>();
    }

    [DataContract]
    public class Points
    {
        [DataMember]
        public int X;

        [DataMember]
        public int Z;

        [DataMember]
        public int Steps;
    }
}
